#ifndef RSIM_VECT6_H
#define RSIM_VECT6_H

/* --------------------------------------------------------------------------*
* File: Vect6.h                                                              *
* Authors: Kishor Bhalerao                                                   *
* Email : kishor8dm@gmail.com                                                *
* Contributors:                                                              *
* Email:                                                                     *
* copyright (c) 2010 Authors.                                                *
*                                                                            *
* This program is free software: you can redistribute it and/or modify it    *
* under the terms of the GNU General Public License as published by the Free *
* Software Foundation, either version 3 of the License, or any later version.*
*                                                                            *
* This program is distributed in the hope that it will be useful, but        *
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY *
* or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License   *
* for more details. (http://www.gnu.org/licenses/)                           *
* -------------------------------------------------------------------------- */

#include "Vector.h"

namespace RSIM{

template <class T> class Matrix_;
template <class T> class Vect3_;

template <class T>
class Vect6_:public Vector_<T>{
        public:
                /** Allocate space */
                Vect6_();
                
                explicit Vect6_(const T& val);
                
                explicit Vect6_(const T *v);
                
                /** Copy constructor */
                Vect6_(const Vect6_<T>&);
                
                /** Copy constructor */
                Vect6_(const Matrix_<T>&);
                
                /** Copy constructor */
                Vect6_(const Vector_<T>&);
                
                void copy(const Vect6_<T>& V);
                
                void copy(const T *v);
                
                void add(const Vect6_<T>& V);
                
                void add(const T *v);
                
                /** Create vector from 2 Vect3's */
                Vect6_(const Vect3_<T>& V1, const Vect3_<T>& V2);
                
                /**
                This function is written with spatial vectors in mind. 
                if you have a 6x1 vector V = [w v]', then getVect3(0,v3)
                will set v3 = w, and getVect3(1,v3) will set v3 = v. */
                void getVect3(const int& index,Vect3_<T>& V)const;

                /** Similar to getVect3(). Vect3 V is copied in the specified index */
                void setVect3(const int& index,const Vect3_<T>& V);

};

template <class T>
inline void Vect6_<T>::copy(const Vect6_<T>& V){
        const T *v = V.Ptr();
        this->data_[0] = v[0];	this->data_[1] = v[1];	this->data_[2] = v[2];
        this->data_[3] = v[3];	this->data_[4] = v[4];	this->data_[5] = v[5];
}
                
template <class T>		
inline void Vect6_<T>::copy(const T *v){
        this->data_[0] = v[0];	this->data_[1] = v[1];	this->data_[2] = v[2];
        this->data_[3] = v[3];	this->data_[4] = v[4];	this->data_[5] = v[5];
}

template <class T>
inline void Vect6_<T>::add(const Vect6_<T>& V){
        const T *v = V.Ptr();
        this->data_[0] += v[0];	this->data_[1] += v[1];	this->data_[2] += v[2];
        this->data_[3] += v[3];	this->data_[4] += v[4];	this->data_[5] += v[5];
}
                
template <class T>		
inline void Vect6_<T>::add(const T *v){
        this->data_[0] += v[0];	this->data_[1] += v[1];	this->data_[2] += v[2];
        this->data_[3] += v[3];	this->data_[4] += v[4];	this->data_[5] += v[5];
}

////////////////////////////////////////////////////////////////////////

template <class T>
Vect6_<T>::Vect6_(const Vect6_<T>& V):Vector_<T>(6){
        const T *ptr = V.Ptr();
        this->data_[0]=ptr[0];
        this->data_[1]=ptr[1];
        this->data_[2]=ptr[2];
        this->data_[3]=ptr[3];
        this->data_[4]=ptr[4];
        this->data_[5]=ptr[5];
        this->isColVec_ = V.isColVector_();
}

////////////////////////////////////////////////////////////////////////

template <class T>
Vect6_<T>::Vect6_(const Vector_<T>& V):Vector_<T>(6){
        const T *ptr = V.Ptr();
        this->data_[0]=ptr[0];
        this->data_[1]=ptr[1];
        this->data_[2]=ptr[2];
        this->data_[3]=ptr[3];
        this->data_[4]=ptr[4];
        this->data_[5]=ptr[5];
        this->isColVec_ = V.isColVector();
}

////////////////////////////////////////////////////////////////////////

template <class T>
Vect6_<T>::Vect6_(const Matrix_<T>& M):Vector_<T>(6){
        #ifdef RSIM_SAFE
        if(M.getNElements()!=6){
                cout<<"\nDimension mismatch: Vect6_<T>::Vect6_(const Matrix_& M). Exiting...";
                exit(0);
        }
        #endif
        const T *ptr = M.Ptr();
        this->data_[0]=ptr[0];
        this->data_[1]=ptr[1];
        this->data_[2]=ptr[2];
        this->data_[3]=ptr[3];
        this->data_[4]=ptr[4];
        this->data_[5]=ptr[5];
        this->isColVec_ = (M.getNCol()==1);
}

////////////////////////////////////////////////////////////////////////

template <class T>
Vect6_<T>::Vect6_(const Vect3_<T>& V1, const Vect3_<T>& V2):Vector_<T>(6){
        const T *v1 = V1.Ptr();
        const T *v2 = V2.Ptr();
        for(int i=0;i<3;i++){
                this->data_[i]=v1[i];
                this->data_[i+3]=v2[i];
        }
}
                
////////////////////////////////////////////////////////////////////////

template <class T>
void Vect6_<T>::getVect3(const int& index,Vect3_<T>& V)const{
        T *Ptr = V.wPtr();
        Ptr[0]=this->data_[index*3]; Ptr[1]=this->data_[index*3+1]; Ptr[2]=this->data_[index*3+2];
}

////////////////////////////////////////////////////////////////////////

template <class T>
void Vect6_<T>::setVect3(const int& index,const Vect3_<T>& V){
        const T *ptr = V.Ptr();
        this->data_[index*3] = ptr[0];
        this->data_[index*3+1] = ptr[1];
        this->data_[index*3+2] = ptr[2];
}

////////////////////////////////////////////////////////////////////////

template <class T>
Vect6_<T>::Vect6_():Vector_<T>(6){}

////////////////////////////////////////////////////////////////////////

template <class T>
Vect6_<T>::Vect6_(const T& val):Vector_<T>(6,val){}

////////////////////////////////////////////////////////////////////////

template <class T>
Vect6_<T>::Vect6_(const T *v):Vector_<T>(6,v){}

} // namespace RSIM

#endif
